#include <iostream>
#include <string>
#include <pcl/io/ply_io.h>  
#include <pcl/io/pcd_io.h>
#include <pcl/io/vtk_lib_io.h> 
#include <pcl/registration/ndt.h> 
#include <pcl/point_types.h> 
#include <pcl/registration/icp.h> 
#include <pcl/visualization/pcl_visualizer.h> 
#include <pcl/console/time.h>   
#include <pcl/filters/voxel_grid.h> 
#include <pcl/common/transforms.h> 
#include <pcl/io/vtk_lib_io.h> 
#include <vector>
#include <pcl/point_cloud.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <pcl/keypoints/sift_keypoint.h>
#include <math.h>

using namespace std;


typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

namespace pcl
{
   template<>
   struct SIFTKeypointFieldSelector<PointXYZ>
   {
       inline float
           operator () (const PointXYZ& p) const
       {
           return p.z;
       }
   };
}

bool get_point(const string& filename, PointCloudT::Ptr points_cloud)
{
   ifstream fs;
   fs.open(filename.c_str(), ios::binary);
   if (!fs.is_open() || fs.fail())
   {
       fs.close();
       return (false);
   }

   string line;
   vector<string> st;

   while (!fs.eof())
   {
       getline(fs, line);
       // Ignore empty lines
       if (line.empty())
           continue;

       // Tokenize the line
       boost::trim(line);
       boost::split(st, line, boost::is_any_of("\t\r "), boost::token_compress_on);

       if (st.size() != 3)
           continue;

       pcl::PointXYZ point;
       point.x = double(atof(st[0].c_str()));
       point.y = double(atof(st[1].c_str()));
       point.z = double(atof(st[2].c_str()));
       points_cloud->push_back(point);
   }
   fs.close();

   return (true);
}

bool save_txt(const string& filename, const vector<PointT>& points_vector) {
	ofstream ofs;
	ofs.open(filename);
	ofs.setf(ios::fixed, ios::floatfield);  // �趨Ϊ fixed ģʽ����С�����ʾ������
	ofs.precision(4);  // ���þ���
	if (!ofs.is_open() || ofs.fail())
	{
		ofs.close();
		return (false);
	}
	for (auto point : points_vector) {
		ofs << point.x << " " << point.y << " " << point.z << endl;
	}
	return true;
}

int main(int argc, char* argv[])
{
   if (argc != 20)
   {
       printf("{\"success\":false}");
       return (0);
   }

   PointCloudT::Ptr cloud_in(new PointCloudT);//��ʼ����
  	PointCloudT::Ptr cloud_aim(new PointCloudT);//Ŀ�����
  	PointCloudT::Ptr cloud_out(new PointCloudT);//�������
  
  	PointCloudT::Ptr cloud_aim_filter(new PointCloudT);//Ŀ�����
  	PointCloudT::Ptr cloud_out_filter(new PointCloudT);//�������
  
  	PointCloudT::Ptr cloud_aim_transformed(new PointCloudT);//Ŀ�����
  	PointCloudT::Ptr cloud_out_transformed(new PointCloudT);//�������
  
  	string filename1 = argv[1];
  	string filename2 = argv[2];

   Eigen::Matrix<double, 4, 4> matrix_rotation;
   for (int i = 0; i < 4; ++i) {
       for (int j = 0; j < 4; ++j) {
           matrix_rotation(i, j) = atof(argv[4 * i + j + 4]);
       }
   }
  
  	pcl::VoxelGrid<pcl::PointXYZ> filter;
  	filter.setLeafSize(0.3f, 0.3f, 0.3f);
  
  	pcl::PolygonMesh mesh;
  	pcl::io::loadPolygonFile(filename2, mesh);
  	fromPCLPointCloud2(mesh.cloud, *cloud_in);
   pcl::transformPointCloud(*cloud_in, *cloud_in, matrix_rotation);

  	filter.setInputCloud(cloud_in);
  	filter.filter(*cloud_out_filter);
  
  	*cloud_out = *cloud_in;
  
  	pcl::io::loadPolygonFile(filename1, mesh);
  	fromPCLPointCloud2(mesh.cloud, *cloud_aim);
  	filter.setInputCloud(cloud_aim);
  	filter.filter(*cloud_aim_filter);



   int iterations = atoi(argv[3]);

	pcl::IterativeClosestPoint<PointT, PointT> icp;   
	icp.setMaximumIterations(iterations);
	icp.setEuclideanFitnessEpsilon(0.0001);
	icp.setInputSource(cloud_out_filter);      
	icp.setInputTarget(cloud_aim_filter);		
	icp.align(*cloud_out_filter);				

	if (!icp.hasConverged())
	{
		printf("{\"success\":false}");
		return (0);
	}
	Eigen::Matrix4d transformation_matrix = icp.getFinalTransformation().cast<double>();
	pcl::transformPointCloud(*cloud_out, *cloud_out, icp.getFinalTransformation());


   //���� sift ��ز���
   const float    min_scale = 0.5;             //���ó߶ȿռ�����С�߶ȵı�׼ƫ��          
   const int      n_octaves = 6;               //���ø�˹�������飨octave������Ŀ            
   const int      n_scales_per_octave = 4;     //����ÿ�飨octave������ĳ߶�  
   const float    min_contrast = 0.01;          //�������ƹؼ��������ֵ


   pcl::SIFTKeypoint<pcl::PointXYZ, pcl::PointWithScale> sift;//����sift�ؼ��������
   pcl::PointCloud<pcl::PointWithScale> result;
   sift.setInputCloud(cloud_aim);//�����������
   pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
   sift.setSearchMethod(tree);//����һ���յ�kd������tree�����������ݸ�sift������
   sift.setScales(min_scale, n_octaves, n_scales_per_octave);//ָ�������ؼ���ĳ߶ȷ�Χ
   sift.setMinimumContrast(min_contrast);//�������ƹؼ��������ֵ
   sift.compute(result);//ִ��sift�ؼ����⣬��������result

   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZ>);
   copyPointCloud(result, *cloud_temp);//��������pcl::PointWithScale������ת��Ϊ������pcl::PointXYZ������

   //ŷ�Ͼ����ж���Ӧ�㡾�ɿ���kd���Ż���
   vector<int> refer_points_index(cloud_temp->size(),-1);
   for (int i = 0; i < cloud_temp->size(); ++i) {
       double min = 100000;
       int min_index = 0;
       for (int j = 0; j < cloud_out->size(); ++j) {
           double temp_min = sqrt(pow((cloud_temp->points[i].x - cloud_out->points[j].x), 2) + pow((cloud_temp->points[i].y - cloud_out->points[j].y), 2) + pow((cloud_temp->points[i].z - cloud_out->points[j].z), 2));
           if (temp_min < min) {
               min = temp_min;
               min_index = j;
           }
       }
       refer_points_index[i] = min_index;
   }

   vector<vector<PointT>> refer_points(2, vector<PointT>(cloud_temp->size()));

   for (int i = 0; i < refer_points_index.size(); ++i) {
       double min = 100000;
       int min_index = 0;
       for (int j = 0; j < cloud_aim->size(); ++j) {
           double temp_min = sqrt(pow((cloud_out->points[refer_points_index[i]].x - cloud_aim->points[j].x), 2) + pow((cloud_out->points[refer_points_index[i]].y - cloud_aim->points[j].y), 2) + pow((cloud_out->points[refer_points_index[i]].z - cloud_aim->points[j].z), 2));
           if (temp_min < min) {
               min = temp_min;
               min_index = j;
           }
       }
       refer_points[0][i] = cloud_in->points[refer_points_index[i]];
       refer_points[1][i] = cloud_aim->points[min_index];
   }

   filename1 = filename1.substr(0, filename1.length() - 4) + "_refer.txt";
   filename2 = filename2.substr(0, filename2.length() - 4) + "_refer.txt";

   if (!save_txt(filename1, refer_points[0]) || !save_txt(filename2, refer_points[1])) {
       printf("{\"success\":false}");
       return (0);
   }

   printf("{");
    printf("\"success\":true");
    printf(",\"filenames\":[\"%s\",\"%s\"]",filename1.c_str(),filename2.c_str());

    printf(",\"matrix\":[");
	printf("%6.3f,%6.3f,%6.3f,%6.3f,", transformation_matrix(0, 0), transformation_matrix(0, 1), transformation_matrix(0, 2), transformation_matrix(0, 3));
	printf("%6.3f,%6.3f,%6.3f,%6.3f,", transformation_matrix(1, 0), transformation_matrix(1, 1), transformation_matrix(1, 2), transformation_matrix(1, 3));
	printf("%6.3f,%6.3f,%6.3f,%6.3f,", transformation_matrix(2, 0), transformation_matrix(2, 1), transformation_matrix(2, 2), transformation_matrix(2, 3));
	printf("%6.3f,%6.3f,%6.3f,%6.3f", (double)0, (double)0, (double)0, (double)1);
	printf("]");

    printf("}\n");
	return (0);  
}